package it.gius.pe.algorithm.gjk.shapes;


import org.jbox2d.common.Transform;
import org.jbox2d.common.Vec2;

import it.gius.pe.Shape.Ellipse;
import it.gius.pe.Shape.witness.EllipsePointWitness;
import it.gius.pe.Shape.witness.Witness;
import it.gius.pe.algorithm.gjk.IGjkContainedShape;
import it.gius.pe.algorithm.gjk.Simplex;

public class GjkEllipse extends GjkSingleShape implements IGjkContainedShape{

	private Ellipse ellipse;

	private Transform transf;

	private Vec2 startingPoint;

	private Simplex simplex;
	
	private EllipsePointWitness witness = null;

	public GjkEllipse(Ellipse ellipse) {
		this.ellipse = ellipse;

		this.transf = new Transform();
		
		witness = new EllipsePointWitness(ellipse.getA());

		startingPoint = new Vec2();

		simplex = new Simplex(Simplex.LARGER_2D_SIMPLEX);
	}


	@Override
	public int getDim() {
		return 15/*(int)((ellipse.getA() + ellipse.getB())/*Math.PI)*/;
	}

	/*@Override
	public void getStartingPoint(int suggestIndex, Vec2 result) {
		transf.set(ellipse.centerPosition, ellipse.angle);
		result.set(ellipse.getA(), 0);
		Transform.mulToOut(transf, result, result);

	}*/
	
	@Override
	public void getStartingPoint(Witness witness, Vec2 result) {
		ellipse.fromWitnessToVec(witness, result);
	}
	
	@Override
	public Witness getWitnessInstance() {
		witness.eX = ellipse.getA();
		witness.eY = 0;
		return witness;
	}

	@Override
	public Simplex getStartingSimplex() {
		getStartingPoint(getWitnessInstance(), startingPoint);
		simplex.vs[0].set(startingPoint);
		simplex.currentDim = Simplex.IS_0_SIMPLEX;
		return simplex;
	}


	@Override
	public Witness supportPoint(Vec2 d, Vec2 supportPoint) {
		transf.set(ellipse.centerPosition, ellipse.angle);
		Vec2 dr = transf.R.mulTrans(d);
		float a = ellipse.getA();
		float b = ellipse.getB();
		float aadrx = a*a*dr.x;
		float bbdry = b*b*dr.y;
		float denom = (float)StrictMath.sqrt(aadrx*dr.x + bbdry*dr.y);

		if(denom != 0)
		{
			//System.out.println("denom == 0");
			supportPoint.x = aadrx/denom;
			supportPoint.y = bbdry/denom;
			//throw new IllegalArgumentException();
			
			
			witness.eX = supportPoint.x; witness.eY = supportPoint.y;
			
			Transform.mulToOut(transf, supportPoint, supportPoint);
		}
		else
		{
			witness.eX = a;
			witness.eY = 0;
		}

		
		return witness;
	}


}
